/*!
  @file local_image_publisher.cpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#include <ros/ros.h>
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <camera_info_manager/camera_info_manager.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/distortion_models.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <fstream>  // NOLINT
#include <string>

using std::string;
using std::ifstream;

namespace image_core 
{

/*! 
 *  @brief  Image publisher 
 *  @author Kyohei Otsu <kyon@ac.jaxa.jp>
 *  @date   2013
 *  
 *  This class publishes images specified by parameter '~filename'
 */
class ImagePublisher 
{
 public:
  ImagePublisher(const ros::NodeHandle &nh, const ros::NodeHandle &cameraL_nh, const ros::NodeHandle &cameraR_nh)
      : nh_(nh)
      , private_nh_("~")
      , cameraL_nh_(cameraL_nh)
      , cameraR_nh_(cameraR_nh)
      , camera_nameL_("camera")
      , camera_nameR_("camera")
      , cinfoL_(new camera_info_manager::CameraInfoManager(cameraL_nh_))
      , cinfoR_(new camera_info_manager::CameraInfoManager(cameraR_nh_))
      , it_(nh) 
  {
    // initializing pub/sub, server
    img_pub_ = it_.advertiseCamera(nh_.resolveName("image_out"), 1);
    imgL_pub_ = it_.advertiseCamera(nh_.resolveName("imageL_out"), 1);
    imgR_pub_ = it_.advertiseCamera(nh_.resolveName("imageR_out"), 1);

    // initializing optional parameters
    // e.g.) private_nh_.param<TYPE>("PARAM_NAME", PARAM_VAR, DEFAULT_VALUE);
    private_nh_.param<std::string>("filename", filename_, "pattern.png");
    private_nh_.param<std::string>("imageL_list", imgL_list_, "imgL");
    private_nh_.param<std::string>("imageR_list", imgR_list_, "imgR");
    private_nh_.param<double>("sim_rate", sim_rate_, 10.0);

    private_nh_.param<std::string>("camera_nameL", camera_nameL_, "left");
    private_nh_.param<std::string>("camera_nameR", camera_nameR_, "right");
    if (!cinfoL_->setCameraName(camera_nameL_) || !cinfoR_->setCameraName(camera_nameR_)) 
      ROS_WARN_STREAM("camera name not valid");

    std::string camera_info_urlL;
    std::string camera_info_urlR;
    private_nh_.param<std::string>("camera_info_urlL", camera_info_urlL, "");
    private_nh_.param<std::string>("camera_info_urlR", camera_info_urlR, "");
    if (camera_info_urlL.length() > 0) cinfoL_->loadCameraInfo(camera_info_urlL);
    if (camera_info_urlR.length() > 0) cinfoR_->loadCameraInfo(camera_info_urlR);

    private_nh_.param<std::string>("camera_frame", camera_frame_, "camera_frame");

#if 0
    std::string calibL, calibR;
    private_nh_.param<std::string>("calibration_fileL", calibL, "");
    private_nh_.param<std::string>("calibration_fileR", calibR, "");
    if (calibL.length() && calibR.length())
    {
      ROS_INFO("Reading calibration files....");
      readCameraInfo(calibL, info_ptrL_);
      readCameraInfo(calibR, info_ptrR_);
    }
#endif
  }

  ~ImagePublisher() { }

  void publishOneImage();

  void publishStereoImages();

 private:
  struct CameraIntParams 
  {
    cv::Mat K;
    cv::Mat D;
  };

  void readCameraInfo(const std::string &calibL, sensor_msgs::CameraInfo::Ptr &cam_info);

  ros::NodeHandle nh_;          //!< ROS node handler
  ros::NodeHandle private_nh_;  //!< ROS node handler in the private namespace
  ros::NodeHandle cameraL_nh_;
  ros::NodeHandle cameraR_nh_;
  std::string camera_nameL_;
  std::string camera_nameR_;
  boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfoL_;
  boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfoR_;
  std::string camera_frame_;
  image_transport::ImageTransport it_;
  image_transport::CameraPublisher img_pub_;
  image_transport::CameraPublisher imgL_pub_;
  image_transport::CameraPublisher imgR_pub_;

  std::string filename_;   //!< image file name
  std::string imgL_list_;  //!< image list name
  std::string imgR_list_;  //!< image list name
  sensor_msgs::CameraInfo::Ptr info_ptrL_;
  sensor_msgs::CameraInfo::Ptr info_ptrR_;

  double sim_rate_;  //!< simuration rate
};

void ImagePublisher::publishOneImage() 
{
  ROS_INFO_STREAM("opening " << filename_);
  cv::Mat image = cv::imread(filename_, 1);
  if (!image.data) {
    ROS_ERROR("failed opening image file");
    ROS_BREAK();
  }

  cv_bridge::CvImage::Ptr cv_ptr(new cv_bridge::CvImage);
  cv_ptr->header.stamp = ros::Time::now();
  cv_ptr->header.frame_id = "camera_frame";
  cv_ptr->encoding = "bgr8";
  cv_ptr->image = image;

  sensor_msgs::CameraInfo::Ptr info_ptr(new sensor_msgs::CameraInfo);
  info_ptr->header.stamp = cv_ptr->header.stamp;
  info_ptr->header.frame_id = cv_ptr->header.frame_id;
  info_ptr->width = cv_ptr->image.cols;
  info_ptr->height = cv_ptr->image.rows;

  img_pub_.publish(cv_ptr->toImageMsg(), info_ptr);
}

void ImagePublisher::publishStereoImages() 
{
  ifstream ifsL(imgL_list_.c_str(), ifstream::in);
  ifstream ifsR(imgR_list_.c_str(), ifstream::in);
  if (!ifsL.is_open() || !ifsR.is_open()) {
    ROS_ERROR("File open");
    ROS_BREAK();
  }

  cv_bridge::CvImage::Ptr cv_ptrL(new cv_bridge::CvImage);
  cv_bridge::CvImage::Ptr cv_ptrR(new cv_bridge::CvImage);
  cv_ptrL->header.frame_id = camera_frame_;
  cv_ptrR->header.frame_id = camera_frame_;
  cv_ptrL->header.stamp = cv_ptrR->header.stamp = ros::Time::now();
  cv_ptrL->encoding = cv_ptrR->encoding = "bgr8";

  sensor_msgs::CameraInfoPtr ciL(new sensor_msgs::CameraInfo(cinfoL_->getCameraInfo()));
  sensor_msgs::CameraInfoPtr ciR(new sensor_msgs::CameraInfo(cinfoR_->getCameraInfo()));
  ciL->header.stamp    = cv_ptrL->header.stamp;
  ciL->header.frame_id = cv_ptrL->header.frame_id;
  ciL->width  = cv_ptrL->image.cols;
  ciL->height = cv_ptrL->image.rows;
  ciR->header.stamp    = cv_ptrR->header.stamp;
  ciR->header.frame_id = cv_ptrR->header.frame_id;
  ciR->width  = cv_ptrR->image.cols;
  ciR->height = cv_ptrR->image.rows;

  string lineL, lineR;
  ros::Rate r(sim_rate_);
  while (getline(ifsL, lineL) && getline(ifsR, lineR)) 
  {
    cv_ptrL->image = cv::imread(lineL, 1);
    cv_ptrR->image = cv::imread(lineR, 1);
    imgL_pub_.publish(cv_ptrL->toImageMsg(), ciL);
    imgR_pub_.publish(cv_ptrR->toImageMsg(), ciR);
    ros::spinOnce();
    r.sleep();
  }
}

void ImagePublisher::readCameraInfo(const std::string &calib_file, sensor_msgs::CameraInfo::Ptr &cam_info)
{
  cv::FileStorage camera_yaml_fs(calib_file, cv::FileStorage::READ);
  if (!camera_yaml_fs.isOpened()) {
    ROS_ERROR("Camera yaml file open falied");
  }

  if(!cam_info)
  {
    cam_info.reset(new sensor_msgs::CameraInfo);
  }

  cam_info->distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
  cv::Mat D;
  camera_yaml_fs["D"] >> D;
  cam_info->D.resize(5);
  for (int i = 0; i < 4; ++i)
    cam_info->D[i] = D.at<float>(i, 0);
  cam_info->D[4] = 0.0;
  
  cv::Mat K;  
  camera_yaml_fs["K"] >> K;
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      cam_info->K[i*3+j] = K.at<float>(i, j);

  cv::Mat R = cv::Mat::eye(3, 3, CV_32F);
  for (int i = 0; i < 3; ++i)
    for (int j = 0; j < 3; ++j)
      cam_info->R[i*3+j] = R.at<float>(i, j);

  cv::Mat P = cv::Mat::eye(3, 4, CV_32F);
  cv::Mat P_roi = P(cv::Rect(0, 0, 3, 3));
  K.copyTo(P_roi);
  std::string id;
  camera_yaml_fs["ID"] >> id;
  if (id == "M6A right") 
  {
    P.at<float>(0, 3) = - P.at<float>(0, 0) * 290;
  }
}


}  // namespace image_core

int main(int ac, char **av) {
  ros::init(ac, av, "ImagePublisher");
  ros::NodeHandle nh;
  ros::NodeHandle cameraL_nh(nh.resolveName("left"));
  ros::NodeHandle cameraR_nh(nh.resolveName("right"));
  image_core::ImagePublisher handler(nh, cameraL_nh, cameraR_nh);
  ros::Duration(3.0).sleep();
  // handler.publishOneImage();
  handler.publishStereoImages();
  ros::spin();
  return 0;
}
